#include "bsp_c610.h"


motor_info_t motor_left = {
	.can_id = LEFT_MOTOR_ID,
};
motor_info_t motor_right = {
	.can_id = RIGHT_MOTOR_ID,
};



int32_t c610_get_speed(uint16_t std_id)
{
	if( std_id == motor_left.can_id )
	{
		return motor_left.vrpm;
	}
	else if( std_id == motor_right.can_id )
	{
		return motor_right.vrpm;
	}
	else
	{
		return 0;
	}
}


//unit:round
int32_t c610_get_mileage(uint16_t std_id)
{
	if( std_id == motor_left.can_id )
	{
		return motor_left.circle_cnt;
	}
	else if( std_id == motor_right.can_id )
	{
		return motor_right.circle_cnt;
	}
	else
	{
		return 0;
	}
}


void c610_can_recv_callback(uint16_t std_id,uint8_t *data)
{
	union u16_u8
	{
		uint16_t u16_;
		uint8_t u8_[2];
	}tmp;
	int16_t delta;
	
	if( std_id == motor_left.can_id )
	{
		motor_left.last_ecd_angle = motor_left.ecd_angle;
		
		tmp.u8_[0] = data[1];
		tmp.u8_[1] = data[0];
		motor_left.ecd_angle = tmp.u16_;
		
		tmp.u8_[0] = data[3];
		tmp.u8_[1] = data[2]; 
		motor_left.vrpm = (int16_t)tmp.u16_;

		tmp.u8_[0] = data[5];
		tmp.u8_[1] = data[4];
		motor_left.moment = (int16_t)tmp.u16_;

		if( motor_left.last_ecd_angle != 0xffff) 
		{			
			if( (motor_left.ecd_angle - motor_left.last_ecd_angle) >= (M2006_ANGLE_MAX+1)/2 )
			{
				delta = motor_left.ecd_angle - M2006_ANGLE_MAX - motor_left.last_ecd_angle;
			}
			else if( (motor_left.ecd_angle - motor_left.last_ecd_angle) <= -(M2006_ANGLE_MAX+1)/2 )
			{
				delta = M2006_ANGLE_MAX - motor_left.last_ecd_angle + motor_left.ecd_angle;
			}
			else
			{
				delta = motor_left.ecd_angle - motor_left.last_ecd_angle;
			}
			motor_left.mileage += delta;			
			motor_left.circle_cnt = motor_left.mileage / M2006_ANGLE_MAX;		
		}		
	}
	else if( std_id == motor_right.can_id )
	{
		motor_right.last_ecd_angle = motor_right.ecd_angle;
		
		tmp.u8_[0] = data[1];
		tmp.u8_[1] = data[0];
		motor_right.ecd_angle = tmp.u16_;
		
		tmp.u8_[0] = data[3];
		tmp.u8_[1] = data[2]; 
		motor_right.vrpm = (int16_t)tmp.u16_;

		tmp.u8_[0] = data[5];
		tmp.u8_[1] = data[4];
		motor_right.moment = (int16_t)tmp.u16_;	
		
		if( motor_right.last_ecd_angle != 0xffff) 
		{			
			if( (motor_right.ecd_angle - motor_right.last_ecd_angle) >= (M2006_ANGLE_MAX+1)/2 )
			{
				delta = motor_right.ecd_angle - M2006_ANGLE_MAX - motor_right.last_ecd_angle;
			}
			else if( (motor_right.ecd_angle - motor_right.last_ecd_angle) <= -(M2006_ANGLE_MAX+1)/2 )
			{
				delta = M2006_ANGLE_MAX - motor_right.last_ecd_angle + motor_right.ecd_angle;
			}
			else
			{
				delta = motor_right.ecd_angle - motor_right.last_ecd_angle;
			}
			motor_right.mileage += delta;			
			motor_right.circle_cnt = motor_right.mileage / M2006_ANGLE_MAX;		
		}		
	}
}



